<template>
  <el-card ref="card">
    <template #header>
      <el-row class="mapView">
        <el-col :span="5">
          <el-space wrap>
            <el-button type="primary" @click="toggleTeleopMode">摇杆</el-button>
            <el-button v-if="!teleopJoystick" type="primary" @click="forceStop"
              >急停</el-button
            >
            <el-button type="primary" @click="refreshMap">连接</el-button>
            <el-button type="primary" :icon="Rank" @click="moveMap"
              >移动</el-button
            >
            <el-button
              type="primary"
              :icon="Upload"
              @click="dialogVisible = true"
            >
              保存
            </el-button>
            <el-dialog
              v-model="dialogVisible"
              title="请输入保存的名称"
              :width="300"
            >
              <el-input
                v-model="map_Name"
                placeholder="请输入名称"
                style="margin-bottom: 20px"
              />
              <template v-slot:footer>
                <div class="dialog-footer" style="text-align: right">
                  <el-button size="small" @click="dialogVisible = false"
                    >取 消</el-button
                  >
                  <el-button type="danger" size="small" @click="save_map"
                    >确 定</el-button
                  >
                </div>
              </template>
            </el-dialog>
          </el-space>
        </el-col>
        <span class="slider-demo-block">缩放：</span>
        <el-col :span="5">
          <el-slider
            v-model="zoomLevel"
            show-input
            :min="0.1"
            :max="5"
            :step="0.1"
            @input="handleSliderChange"
          />
        </el-col>
        <el-button type="warning" :icon="Refresh" @click="resetMap"
          >重置</el-button
        >
        <el-col :span="1" />
        <el-col :span="4">
          <el-button type="warning" @click="startSlam">建图</el-button>
          <el-button type="danger" :icon="Close" @click="cleanNodes"
            >关闭</el-button
          >
        </el-col>
      </el-row>
    </template>
    <div class="map-container">
      <div id="map" />
    </div>
    <div class="virtual-gamepad-teleop" />
  </el-card>
</template>

<script setup lang="ts">
const ROSLIB = window.ROSLIB;
const ROS2D = window.ROS2D;
const NAV2D = window.NAV2D;
import nipplejs from "nipplejs";
import { onMounted, ref, reactive, watch } from "vue";
import ROSCommand from "@/api/user";
import { ElMessage } from "element-plus";
import { ip } from "@/api/utils";
import { Refresh, Upload, Rank, Close } from "@element-plus/icons-vue";

const onMapping = ref(false);
const card = ref(null);
// ROS Related
let ros2 = undefined;
let map_data = undefined;
const globalState = reactive({
  height: 0,
  width: 0,
  zoomscale: 1
});
let isMoving = false;
let isPanning = false;
const zoomLevel = ref(1);
let viewer = undefined;
let zoomView = undefined;
let panView = undefined;
let initialPose = null;
let navigator = null;

const dialogVisible = ref(false);
const rosCommandInstance = new ROSCommand();

const initializeROSCommunication = () => {
  rosCommandInstance.Command(
    "roslaunch",
    "rosbridge_server rosbridge_websocket.launch"
  );
  rosCommandInstance.Command(
    "rosrun",
    "tf2_web_republisher tf2_web_republisher"
  );
};
const handleSliderChange = value => {
  globalState.zoomscale = value;
};

const resetMap = () => {
  zoomView.startZoom(
    card.value.$el.offsetWidth / 2,
    card.value.$el.offsetHeight / 2
  );
  console.log("zoom:", globalState.zoomscale);
  zoomView.zoom(1);
  globalState.zoomscale = 1;
  zoomLevel.value = 1;
};

const startSlam = () => {
  rosCommandInstance.Command(
    "python3",
    "~/test_ws/src/race_navigation/scripts/startSlam.py"
  );
};

const moveMap = () => {
  isMoving = true;
  if (navigator) {
    navigator.destroy();
    navigator = null;
  }
  if (initialPose) {
    initialPose.destroy();
    initialPose = null;
  }
  viewer.scene.on("stagemousedown", function (event) {
    if (isMoving) {
      isPanning = true;
      panView.startPan(event.stageX, event.stageY);
      zoomView.startZoom(event.stageX, event.stageY);
    }
  });

  viewer.scene.on("stagemousemove", function (event) {
    if (isMoving && isPanning) {
      panView.pan(event.stageX, event.stageY);
    }
  });

  viewer.scene.on("stagemouseup", function () {
    if (isMoving) {
      isPanning = false;
    }
  });
};

const map_Name = ref("");
const save_map = () => {
  rosCommandInstance.Command(
    "rosrun",
    `map_server map_saver -f ~/map/${map_Name.value}`
  );
  ElMessage({
    message: "地图保存成功!",
    type: "success"
  });
  dialogVisible.value = false;
};

const cleanNodes = () => {
  ros2.getNodes(nodes => {
    console.log("Active nodes:");
    nodes.forEach(node => {
      console.log(node);
      rosCommandInstance.Command("rosnode", `kill ${node}`);
    });
  });
};

onMounted(() => {
  globalState.width = card.value.$el.offsetWidth;
  globalState.height = card.value.$el.offsetHeight;
  ros2 = new ROSLIB.Ros({
    url: "ws://" + ip("9090")
  });
  ros2.on("error", function (error) {
    console.error("Failed to connect to ROS: ", error);
    ElMessage.error("ROS连接失败!");
  });
  map_data = new ROSLIB.Topic({
    ros: ros2,
    name: "/map",
    messageType: "nav_msgs/msg/OccupancyGrid"
  });
  viewer = new ROS2D.Viewer({
    divID: "map",
    width: globalState.width,
    height: globalState.height
  });
  zoomView = new ROS2D.ZoomView({
    rootObject: viewer.scene
  });
  panView = new ROS2D.PanView({ rootObject: viewer.scene });
  map_data.subscribe(map => {
    if (
      globalState.height != map.info.height ||
      globalState.width != map.info.width
    ) {
      globalState.height = map.info.height;
      globalState.width = map.info.width;
      viewer.width = globalState.width;
      viewer.height = globalState.height;
      zoomView.startZoom(
        card.value.$el.offsetWidth / 2,
        card.value.$el.offsetHeight / 2
      );
    }
  });

  watch(
    () => [globalState.zoomscale],
    ([newScale]) => {
      zoomView.zoom(newScale);
    }
  );

  new NAV2D.OccupancyGridClientNav({
    ros: ros2,
    serverName: "/move_base",
    continuous: true,
    rootObject: viewer.scene,
    viewer: viewer,
    withOrientation: true
  });
});

// Joystick Related
const teleopJoystick = ref(true);
let teleopPublisher = undefined;
let teleopGamepad = undefined;
const toggleTeleopMode = () => {
  if (teleopJoystick.value) {
    teleopGamepad = nipplejs.create({
      zone: document.querySelector(".virtual-gamepad-teleop"),
      mode: "static",
      position: { left: "85%", top: "85%" },
      color: "#0072FF",
      threshold: 0.0,
      fadeTime: 400,
      maxNumberOfNipples: 1
    });

    teleopPublisher = new ROSLIB.Topic({
      ros: ros2,
      name: "/cmd_vel",
      messageType: "geometry_msgs/msg/Twist"
    });

    teleopGamepad.on("move", function (event, data) {
      const r = data.distance / 200;
      const theta = data.angle.radian;
      const cmdVelData = new ROSLIB.Message({
        linear: {
          x: r * Math.sin(theta)
        },
        angular: {
          z: -r * Math.cos(theta)
        }
      });

      teleopPublisher.publish(cmdVelData);
    });
  } else {
    if (teleopGamepad != undefined) {
      const cmdVelData = new ROSLIB.Message({
        linear: {
          x: 0
        },
        angular: {
          z: 0
        }
      });
      teleopPublisher.publish(cmdVelData);
      teleopGamepad.destroy();
    }
  }
  teleopJoystick.value = !teleopJoystick.value;
};

// Force Stop Related
const forceStop = () => {
  const cmdVelData = new ROSLIB.Message({
    linear: {
      x: 0
    },
    angular: {
      z: 0
    }
  });
  teleopPublisher.publish(cmdVelData);
};

const refreshMap = () => {
  initializeROSCommunication();
  onMapping.value = true;
};
</script>

<style lang="scss" scoped>
.flex_r_b {
  display: flex;
  justify-content: space-between;
  align-items: center;
}

.mx-1 {
  position: absolute;
  right: 0;
  top: 0;
  width: auto;
  height: auto;
  border: 3px solid var(--el-border-color);
}

.map-container {
  width: 70vw;
  height: 75vh;
  position: relative;
}

.el-dialog__wrapper[fullscreen="true"] .el-dialog {
  width: 100% !important;
  height: 100% !important;
  max-height: none !important;
  margin: 0 !important;
}

.dialog-header {
  display: flex;
  justify-content: space-between;
  align-items: center;
}

.dialog-fullscreen-button {
  color: #606266;
}

.canvas-container {
  display: flex;
  justify-content: center;
  align-items: center;
  height: 100%;
}

.centered-canvas {
  border: 1px solid black;
  width: 80%;
  height: 80%;
}

.slider-demo-block {
  max-width: 600px;
  display: flex;
  align-items: center;
}
</style>
